#include <stdio.h>
#include "math.h"
#define PI 3.14159265358979323846

typedef struct {
    double l1;  // 基座到肩部
    double l2; // 肩部到肘部
    double l3;  // 肘部到腕部
    double l4;  // 腕部到末端1
    double l5;
} ArmDimensions;

// 关节角度结构体（单位：弧度）
typedef struct {
    double theta1;  // 基座旋转
    double theta2;  // 肩部俯仰
    double theta3;  // 肘部俯仰
    double theta4;  // 腕部俯仰
    double theta5;  // 末端旋转
    double theta6;  // 末端夹持
} JointAngles;

// 末端位姿结构体
typedef struct {
    double x, y, z;  // 位置（mm）
    double pitch;    // 俯仰角ψ（弧度）
    double roll;     // 翻滚角φ（弧度）
} EndEffectorPose;
int inverseKinematics(const ArmDimensions* dim, EndEffectorPose* pose, JointAngles* angles);


int main(){

    JointAngles solution;
    
    ArmDimensions myArm = {
        // .l1 = 91.5f,  
        // .l2 = 80.0f,
        // .l3 = 100.0f,
        // .l4 = 140.0f,

        .l1 = 0.0f,  
        .l2 = 80.0f,
        .l3 = 100.0f,
        .l4 = 50.0f,
        .l5 = 90.0f

        
    };
    
    EndEffectorPose target = {
        .x = 131.947922,
		.y = 131.947922,
		.z = 119.282032,
		.pitch = 0,  
		.roll = 0   
    };


    if (inverseKinematics(&myArm, &target, &solution) == 1) {
        printf("Solution found:\n");
        printf("Theta1: %f deg\n", round(solution.theta1 * 180/PI));
        printf("Theta2: %f deg\n", round(solution.theta2 * 180/PI));
        printf("Theta3: %f deg\n", round(solution.theta3 * 180/PI));
        printf("Theta4: %f deg\n", round(solution.theta4 * 180/PI));
        printf("Theta5: %f deg\n", round(solution.theta5 * 180/PI));
    } else {
        printf("No solution exists for this pose.\n");
        printf("Theta1: %f deg\n", solution.theta1 * 180/PI);
        printf("Theta2: %f deg\n", solution.theta2 * 180/PI);
        printf("Theta3: %f deg\n", solution.theta3 * 180/PI);
        printf("Theta4: %f deg\n", solution.theta4 * 180/PI);
        printf("Theta5: %f deg\n", solution.theta5 * 180/PI);
    }

    return 0;
}
int inverseKinematics(const ArmDimensions* dim, EndEffectorPose* pose, JointAngles* angles) {
    // 步骤1：求解θ1 (基座旋转)
    angles->theta1 =atan2(pose->y, pose->x);

    // 中间变量计算
    double psi = pose->pitch *PI/180;
    
    double phi = pose->roll *PI/180;
    
    //计算A, B, C
    double c1 = cosf(angles->theta1);
    double s1 = sinf(angles->theta1);
    
    double term1 = -pose->x * c1 - pose->y * s1;
    double term2 = (dim->l4 + dim->l5) * sinf(psi - PI/2);

    double A = term1 - term2 + dim->l1;
    double B = pose->z - (dim->l4 + dim->l5) * cosf(psi - PI/2);
    double C = (A*A + B*B + dim->l2*dim->l2 - dim->l3*dim->l3) / (2 * dim->l2);

    
    // 步骤2：求解θ2 (肩部俯仰)
    double t;
    printf("B + C=%f\n",B + C);
    if (fabsf(B + C) == 0  && A!=0) {
        t = -(B - C) / (2 * A);
    } 
    else {
        double sqrt_term = A*A + B*B - C*C;
        printf("sqrt_term=%f\n",sqrt_term);
        if (sqrt_term < 0) return 0;  // 无解
        
        t = (A + sqrtf(sqrt_term)) / (B + C);  // 选择"近端"解
    }
    angles->theta2 = 2 * atan(t);/////////
    
    // 步骤3：求解θ23 = θ2 + θ3
    double s2 = sinf(angles->theta2);
    double c2 = cosf(angles->theta2);
    
    double s23 = (A - dim->l2 * s2) ;
    double c23 = (B - dim->l2 * c2) ;
    double theta23 = atan2(s23, c23);
    
    // 步骤4：求解θ3和θ4
    angles->theta3 = theta23 - angles->theta2;
    angles->theta4 = psi - theta23 - PI/2;
    
    // 步骤5：θ5直接等于翻滚角φ
    
    angles->theta5 = phi;
    
    return 1;

    // angles->theta1 = atan2(pose->y, pose->x);

    // double theta234 = pose->pitch + PI/2;
    // double len = sqrtf(pose->x*pose->x+pose->y*pose->y);
    // double high = pose->z;
    // double L = len - arm->l4 * sin(theta234);
    // double H = high - arm->l1 - arm->l4 * cos(theta234);

    // double c3 = (L*L + H*H - arm->l2*arm->l2 - arm->l3*arm->l3)/2 * arm->l2*arm->l3;
    // printf("%f\n",c3);
    // printf("%f\n",1-c3*c3);

    // if(1-c3*c3 < 0) return 0;
    // double s3 = sqrt(1-c3*c3);

    // angles->theta3 = atan2(s3,c3);

    // double k1 = arm->l2 + arm->l3*c3;
    // double k2 = arm->l3*s3;

    // double w = atan2(k2,k1);
    // angles->theta2 = atan2(L,H) - w;

    // angles->theta4 = theta234 - angles->theta2 - angles->theta3;

    // angles->theta5 = pose->roll;

    // return 1;

}